#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@Copyright    :   Jihua Lab 2021.
@File    :   dual_arm_cmd_client.py
@Author  :   WANG Hao 
@Contact :   wanghao@jihualab.com、haowanghk@gmail.com(Github)
@License :   (C)Copyright 2020-2021, JHL
@Version :   1.1
@Create Time    :   CURRENT_YEAR/CURRENT_MONTH/CURRENT_DATE CURRENT_HOUR:CURRENT_MINUTE:CURRENT_SECOND
@Modify Time      @Author    @Version    @Desciption
-------------------        --- -------    ---  --------   --- -----------
@File Description :   None 
'''

# here put the import lib

from __future__ import print_function

import sys
import rospy

from sensor_msgs.msg import JointState
from joint_state_exchange.msg import RobotState
from dual_arm_base_control.msg import TelemetryInfo
from std_msgs.msg import String

from dual_arm_base_control.srv import CtrlModeSelect


# loading dual arm control mode parameters into Global variables
G_MODE_LIST = []
G_MODE_LIST.append(rospy.get_param("/control_mode/auto_mode"))
G_MODE_LIST.append(rospy.get_param("/control_mode/teach_mode"))
G_MODE_LIST.append(rospy.get_param("/control_mode/servo_mode"))
G_MODE_LIST.append(rospy.get_param("/control_mode/landing_mode"))

def control_mode_client(mode = 3):
    # default control mode is landing init mode and landing pose
    rospy.wait_for_service('/control_mode_select')
    try:
        control_mode_nh = rospy.ServiceProxy('/control_mode_select', CtrlModeSelect)
        resp1 = control_mode_nh(mode)
        return resp1.result
    except rospy.ServiceException as e:
        rospy.logerr("Service call failed: %s"%e)

def usage():
    return "Control mode %s "%sys.argv[0]

if __name__ == "__main__":
    if len(sys.argv) == 2 and (int(sys.argv[1]) in G_MODE_LIST):
        mode_type = int(sys.argv[1])
        control_mode_client(mode = mode_type)
        rospy.loginfo("------ Control mode %s ------" %sys.argv[0])
    else:
        print(usage())
        sys.exit(1) # exit main function
    rospy.loginfo("Control Mode Selection Service - Requesting mode %s"%(mode_type))
    rospy.loginfo("Control Mode Selection Service - Response  %s result : %s "%(mode_type, 
                                    control_mode_client(mode_type)))